using System;
using System.Collections;
using System.Collections.Generic;
using System.Text;

using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;


using FarseerGames.FarseerXNAPhysics.Mathematics;
using Traffic.Graphics;

namespace Traffic.Path
{
    public class PathNode : AStarNode
    {        
        
        private SimpleBody _state;

        public SimpleBody Body
        {
            get { return _state; }
            set { _state = value; }
        }
        
        public PathNode(PathNode AParent, AStarNode AGoalNode, float ACost, SimpleBody b)            
        {
            this.Body = b;

            this.Parent = AParent;
            this.Cost = ACost;
            this.GoalNode = AGoalNode;
        }
               
        /// <summary>
        /// Creates a successor node of the current node based on the given Action
        /// </summary>
        /// <param name="ASuccessors"></param>
        /// <param name="move"></param>
        private void AddSuccessor(ref ArrayList ASuccessors, SimpleBody.Action act)
        {
            // All cations cost the same
            float currentCost = 0.1f;
            
            SimpleBody b = new SimpleBody();

            b.Position = this.Body.Position;
            b.LinearVelocity = this.Body.LinearVelocity;
            b.Orientation = this.Body.Orientation;
            b.ExecuteAction(act, 0.1f);
           
            PathNode NewNode = new PathNode(this, GoalNode, Cost + currentCost, b);

            ASuccessors.Add(NewNode);
        }

        public override bool Equals(object obj)
        {
            return this.Body.Equals(((PathNode)obj).Body);
            
        }
        /// <summary>
        /// Calculates if the given state is close enough to be the same state as this
        /// </summary>
        /// <param name="ANode"></param>
        /// <returns></returns>
        public override bool IsSameState(AStarNode node)
        {
            if (node == null)
            {
                return false;
            }            

            float dist = Calculator.DistanceBetweenPointAndPoint(((PathNode)node).Body.Position, this.Body.Position);

            if (dist < 2f)
            {
                return true;
            }
            return false;           
        }

        /// <summary>
        /// Calculates the estimated cost for the remaining trip to the goal.
        /// </summary>
        public override void Calculate()
        {
            if (GoalNode != null)
            {
                GoalEstimate = Calculator.DistanceBetweenPointAndPoint(((PathNode)GoalNode).Body.Position, this.Body.Position);          
            }
            else
            {
                GoalEstimate = 0f;
            }
        }

        /// <summary>
        /// Gets all successors nodes from the current node and adds them to the successor list
        /// </summary>
        /// <param name="ASuccessors">List in which the successors will be added</param>
        public override void GetSuccessors(ref ArrayList ASuccessors)
        {
            ASuccessors.Clear();

            AddSuccessor(ref ASuccessors, SimpleBody.Action.ACCELERATE);
            AddSuccessor(ref ASuccessors, SimpleBody.Action.BRAKE);
            AddSuccessor(ref ASuccessors, SimpleBody.Action.TURN_LEFT);
            AddSuccessor(ref ASuccessors, SimpleBody.Action.TURN_RIGHT);
            AddSuccessor(ref ASuccessors, SimpleBody.Action.WAIT);            
        }

        /// <summary>
        /// Prints information about the current node
        /// </summary>
        public override void PrintNodeInfo()
        {
            //Console.WriteLine("X:\t{0}\tY:\t{1}\tCost:\t{2}\tEst:\t{3}\tTotal:\t{4}", FX, FY, Cost, GoalEstimate, TotalCost);
        }

    
    }
}

